#ifndef __MYIMU_H_
#define __MYIMU_H_ 1

#include <math.h>
#include "MyConfig.h"
#include "MySensors.h"

// some constants
#define SMOOTH_SCALE			10		// gia tri on dinh
#define SMOOTH_SCALE_H			5		// gia tri on dinh

#define Kp						2.0f 	// proportional gain governs rate of convergence to accelerometer/magnetometer
#define Ki						0.005f	// integral gain governs rate of convergence of gyroscope biases
//#define kpAcc	0.2f
//#define kiAcc	0.005f
//#define kpMag	2.0f
//#define kiMag	0.005f
float q0, q1, q2, q3;					// quaternion elements representing the estimated orientation
float exInt, eyInt, ezInt;				// scaled integral error
unsigned long lastUpdate, now;			// sample period expressed in milliseconds
float halfT;							// 1/2 the sample period expressed in seconds

extern int32_t h;						// do cao

extern float q[4];
extern float Omega[3];
extern float val[9];					// dung cho ham main truyen du lieu cho PC
//extern float testmag[3];
extern unsigned char setup_status;

extern float HC_Yaw, HC_Pitch, HC_Roll;
void MyIMU_Init();
void MyIMU_AHRSupdate(float ax, float ay, float az, float gx, float gy, float gz, float mx, float my, float mz);
float MyIMU_GetHeading();
void MyIMU_getQuaternion(float * q);
void MyIMU_getYawPitchRoll(float * ypr);
//void MyIMU_getValues(float * values);
//void MyIMU_getEuler(float * angles);

int32_t SmoothH(int32_t val);


//============================//
//============5DOF============//
//============================//

void MyIMU_Init5DOF();
void MyIMU_Init3DOF();
void MyIMU_get5DOF();
void MyIMU_get3DOF();

void getInclination5DOF();
void normalize3DVec(float * vector);
float squared(float x);

float RwAcc[3];		//Projection of normalized gravitation force vector on x/y/z axis, as measured by accelerometer
float Gyro_ds[3];	//Gyro readings
float RwGyro[3];		//Rw obtained from last estimated value and gyro movement
float RwEst[3];		
float Awz[2];		//Angles between projection of R on XZ/YZ plane and Z axis (deg)

int firstSample;
float deltaT;

#define wGyro	10.0f
#define PI		3.141592654f

//=================================//
//============NonONCHIP============//
//=================================//

int Acc[3];
int Gyro[3];

#endif
